/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************
 *  @file x.h
 *  功能:
 *
 *                                                                         *
 ***************************************************************************/
#ifndef MOTIONINSTRUCTIONLOGIC_H
#define MOTIONINSTRUCTIONLOGIC_H
#include "motioninstruction.h"
#include "programset.h"
#include "instructionEnum.h"
#include "robotio.h"
#include "mccontrollerthread.h"
#include "axismoveinstruction.h"
#include "fleetdefine.h"
class GcodeParse;
class ProgramEngine;
/**
 * @brief The MotionInstructionLogic class   指令逻辑处理类 MotionInstructionLogic velocity被缩放了１００
 */
class MotionInstructionLogic
{
public:
    MotionInstructionLogic(QString configPathIn, int robotIdIn, ProgramSet* programSetIn, RobotIo* ioControllerIn,
                           ProgramEngine* programEngineIn, McControllerThread *programMcControllerIn
                           , QVector <PointPro> *getPointProListIn, QVector <PlRegStruct> *regPlListIn);

public:
    E_PROGRAM_LOOP_RESULT bufferMove(int programNum, int startLine, double velIn);
    int addCircle(int idIn, std::vector<double> posIn, std::vector<double> centerIn,
                  std::vector<double> normalIn, double vel, int turnsIn, int velTypeIn);
    E_PROGRAM_LOOP_RESULT addProgramMoveCommand_hmi(vector<MoveParameter>& tmpCommandVectorIn);
    int addMonitorDi_fleet(bool isOr,QVector<int> diIndex,QVector<int> diValue);
    E_PROGRAM_LOOP_RESULT moveRobotArm_run(const QVector<int> &lineList,
                                   const QVector<RobotArmMoveParameter> &pathListParaIn, int isLastPointMove=0);
    E_PROGRAM_LOOP_RESULT startAxisMove(int programNum, int startLine,const AxisMoveInstruction &motionInstruction);
    E_PROGRAM_LOOP_RESULT startMotion(int programNum, int startLine);
    E_PROGRAM_LOOP_RESULT startMotion_Gcode(int programNum, int startLine);
    int clearAllMsg( );
    int reset();
    int getSetMotionBlock(int startLineNum);//not used
    /**
     * @brief pretreatMotion    预处理运动指令;hualei 实际上没起作用
     * @param programNum        程序标号
     * @return
     */
    int pretreatMotion();
    int setLaserTrackConfigParameter(struct LaserTrackConfig paraIn);
    int getLaserTrackConfigParameter(struct LaserTrackConfig &paraOut);

private:
    E_PROGRAM_LOOP_RESULT laserCameraTrackLine(int programNum, int startLine);
    E_PROGRAM_LOOP_RESULT addOnePointToMove(int programNum, int startLine, PointPro targetPointIn, int lineIdIn);
    int processMotionInstruction(int programNum, int startLine);//普通规划
    int processMotionInstruction_carvingTp(int programNum, int startLine);//carving tp
    int processMotionInstruction_gcode(int programNum, int startLine);
    int processAxisMoveInstruction(int startLine,const AxisMoveInstruction &motionInstruction);
    bool isFleetMonitorOk();
    bool isConditionTrig(int diIndex, bool diValue);
    int isMotionInstructionFinished(int programNum,int waitLine);//bug,不再使用．
    int isMotionInstructionFinished(int waitLine);
    int getInsTargetPoint(const MotionInstruction &motion, int &pointTypeOut,
                          int &pointIdOut, PointPro &targetPointout);

    //一直加轨迹。暂停就不能返回，防止下次逻辑处理问题
    E_PROGRAM_LOOP_RESULT addProgramMoveCommand_fleet(const QVector<int> &lineList,
                                                      const QVector<RobotArmMoveParameter> &pathListParaIn);
    //判断如果添加过轨迹，则不在继续加轨迹。
//    E_PROGRAM_LOOP_RESULT addProgramMoveCommand(const MoveFreeInstruction &instructionIn,int lineIndex,
//                                         bool isDirectPoint, DPoint targetPoint,int programNumIn);
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish_fleet(const QVector<int> &lineList);
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish_fleet(int lineIndex);//暂停必须在里面等待。停止退出
    //报错后设置quickStopFlag，轨迹被清空．重新运行该条命令(解决暂停退出会导致增量运动多加一次．)
    E_PROGRAM_LOOP_RESULT waitMotionAccomplish(int lineIndexIn,
                           bool isMonitorDi=false,int diIndex=0,bool diValue=false);
    bool isNeedAddTraj(int programNum, int startLine);
    /**
     * @brief getBestPositionFromAngle
     * @param idealAngle -180~180
     * @param initialPosition 包含多圈的绝对角度
     * @param bestPositionOut　包含多圈的绝对角度
     * @return
     */
    int getBestPositionFromAngle(double idealAngle, double initialPosition, double followAxisStartAngle,
                                 double &bestPositionOut);
    int translatePN180(double &degree);
    double angleRotationJudgement(double targetAngle, double initialAngle);//到目标姿态的最小路径转角
    int writeConfigFile(QString configFileName,LaserTrackConfig laserTrackConfigIn);
    int initialConfigFile(QString configFileName);

    /**
     * @brief addMsg  , 添加消息反馈
     * @param messageLevel
     * @param componentName
     * @param messageType
     * @param messageCode
     * @param robotId
     * @param parameter1
     * @param parameter2
     * @param parameter3
     * @param parameter4
     */
    void addMsg( int messageLevel,
                 std::string componentName,
                 std::string messageType,
                 int messageCode,
                 int robotId,
                 int parameter1 = 0,
                 int parameter2 = 0,
                 int parameter3 = 0,
                 int parameter4 = 0,
                 QString message= "");

private:
    int robotId;
    ProgramSet* programSet;
    RobotIo* ioController;
    ProgramEngine* programEngine;

    /**
     * @brief programMcController       算法线程指针
     */
    McControllerThread *programMcController;
    /**
     * @brief getPointProList   保存普通点到ｐｏｉｎｔＤａｔａ组件的操作
     */
    QVector <PointPro> *getPointProList;

    /**
     * @brief regPlList         对接RegData组件的pl寄存器接口
     */
    QVector <PlRegStruct> *regPlList;

    /**
     * @brief tmpMotionBlock
     */
    QVector <MoveParameter> tmpMotionBlock;//not used

    /**
     * @brief motionParaList        运动指令数据
     */
    QVector <MoveParameter> motionParaList;

    int lastAddedLineId;

//    bool waitFlag;
    QMutex diMuTex;
    bool fleetMonitor_isOr;
    QVector<int> fleetMonitor_diIndex;
    QVector<int> fleetMonitor_diValue;

    int armTpType;
    int maxLineCatshDepth_carvingTp;//最多预处理的函数

    FilteringAlgorithm *filterX;
    FilteringAlgorithm *filterY;
    FilteringAlgorithm *filterZ;

    LaserTrackConfig laserTrackConfig;
    QString configPath;
    GcodeParse *gcodeParse;

};

#endif // MOTIONINSTRUCTIONLOGIC_H
